#!/usr/bin/python3

import sys
import roslib
import rospy
import math
import tf
import tf2_ros
from geometry_msgs.msg import Pose, Twist, TransformStamped, PointStamped, PoseStamped
import time
import numpy as np
import csv
from std_msgs.msg import Float64MultiArray, Bool
from sensor_msgs.msg import LaserScan, JointState
from gazebo_msgs.msg import ModelStates
import copy
from tf2_geometry_msgs import tf2_geometry_msgs
from real_sim_mapping import real_sim_mapping

class real_sim_mapping_agv(real_sim_mapping):
    def __init__(self,nodename,ns_prefix):
        super(real_sim_mapping_agv,self).__init__(nodename)

        debug_prefix = "/" + ns_prefix + "/"
        # debug_prefix = ""

        self.tfBuffer = tf2_ros.Buffer()
        self.tfListener = tf2_ros.TransformListener(self.tfBuffer)

        self.sim_base_link = ns_prefix + "/sim/base_link"
        self.real_base_link = ns_prefix + "/real/base_link"

        self.sim_agv_pose = rospy.Publisher(debug_prefix + "real/sim_agv_pose", PoseStamped, queue_size = 10)
        self.sim_cmd_vel = rospy.Publisher(debug_prefix + "sim/cmd_vel", Twist,queue_size=10)

        self.realtime_tracking_switch_on = False

    def run(self):
        rate = rospy.Rate(20)
        trans = PoseStamped()

        xhist = []
        yhist = []
        zhist = []

        MAX_INT_LENGTH = 10
        PID_I = 0.02
        PID_P = 1.0

        while not rospy.is_shutdown():
            if (not self.tfBuffer.can_transform("world", self.sim_base_link, rospy.Time(0))):
                rate.sleep()
                continue
            
            initPose = self.tfBuffer.lookup_transform('world', self.sim_base_link, rospy.Time(0))

            trans.header.frame_id = self.sim_base_link
            trans.header.stamp = rospy.Time.now()

            trans.pose.position.x = initPose.transform.translation.x
            trans.pose.position.y = initPose.transform.translation.y
            trans.pose.position.z = initPose.transform.translation.z

            trans.pose.orientation.x = initPose.transform.rotation.x
            trans.pose.orientation.y = initPose.transform.rotation.y
            trans.pose.orientation.z = initPose.transform.rotation.z
            trans.pose.orientation.w = initPose.transform.rotation.w

            self.sim_agv_pose.publish(trans)

            if (not self.tfBuffer.can_transform(self.sim_base_link, self.real_base_link, rospy.Time(0))):
                rate.sleep()
                continue

            controlTransform = self.tfBuffer.lookup_transform(self.sim_base_link, self.real_base_link, rospy.Time(0))
            quat = controlTransform.transform.rotation
            euler = tf.transformations.euler_from_quaternion([quat.x, quat.y, quat.z, quat.w],'sxyz')
            deltap = controlTransform.transform.translation
            # print(self.sim_base_link,"x, y, theta:",deltap,euler)

            xhist.append(deltap.x)
            yhist.append(deltap.y)
            zhist.append(euler[2])

            xint = 0.0
            yint = 0.0
            zint = 0.0
            if(len(xhist)>MAX_INT_LENGTH):
                xhist.pop(0)
                yhist.pop(0)
                zhist.pop(0)

                xint = sum(xhist) * PID_I
                yint = sum(yhist) * PID_I
                zint = sum(zhist) * PID_I

            xint = xint + deltap.x * PID_P
            yint = yint + deltap.y * PID_P
            zint = zint + euler[2] * PID_P * 0.2 * 50

            cmd = Twist()
            cmd.linear.x = xint if math.fabs(xint) < 1.0 else math.fabs(xint)/xint * 1.0
            cmd.linear.y = yint if math.fabs(yint) < 1.0 else math.fabs(yint)/yint * 1.0
            cmd.angular.z = zint if math.fabs(zint) < 0.2*50 else math.fabs(zint)/zint * 0.2*50

            self.sim_cmd_vel.publish(cmd)
            
            rate.sleep()

if __name__ == '__main__':
    prefix = "pogo_agv"

    if(len(sys.argv)>1):
        prefix = sys.argv[1]
        print("file written to prefix",prefix)

    service = real_sim_mapping_agv('real_sim_mapping_agv',prefix)
    service.run()
